Private Function CombinedAngle(ByVal a1 As Single, ByVal a2 As Single, ByVal f1 As Single, ByVal f2 As Single) As Single
Const PI = 3.14159265
Const PI_TIMES_2 = PI * 2
If Abs(a1 - a2) > PI Then
If a1 > a2 Then
Do
a1 = a1 - PI_TIMES_2
Loop While a1 > a2
Else
Do
a2 = a2 - PI_TIMES_2
Loop While a2 > a1
End If
End If
CombinedAngle = f1 * a1 + f2 * a2
End Function
' Copy another robot's parameters.
Public Sub CopyFrame(from_me As Robot)
With from_me
Cx = .Cx
Cy = .Cy
LShoulderAngle = .LShoulderAngle
RShoulderAngle = .RShoulderAngle
LElbowAngle = .LElbowAngle
RElbowAngle = .RElbowAngle
LHipAngle = .LHipAngle
RHipAngle = .RHipAngle
LKneeAngle = .LKneeAngle
RKneeAngle = .RKneeAngle
End With
End Sub
' Return the position of part of the robot.
Public Sub Position(part As Integer, x As Integer, y As Integer)
Select Case part
Case part_Head
x = Cx
y = Cy
Case part_Neck
x = Cx
y = Cy + 2 * HeadRadius
Case part_Shoulders
x = Cx
y = Cy + 2 * HeadRadius + NeckLength
Case part_Lelbow
x = Cx + _
UpperArmLength * Cos(LShoulderAngle)
y = Cy + 2 * HeadRadius + NeckLength - _
UpperArmLength * Sin(LShoulderAngle)
Case part_RElbow
x = Cx + _
UpperArmLength * Cos(RShoulderAngle)
y = Cy + 2 * HeadRadius + NeckLength - _
UpperArmLength * Sin(RShoulderAngle)
Case part_LHand
x = Cx + _
UpperArmLength * Cos(LShoulderAngle) + _
LowerArmLength * Cos(LElbowAngle)
y = Cy + 2 * HeadRadius + NeckLength - _
UpperArmLength * Sin(LShoulderAngle) - _
LowerArmLength * Sin(LElbowAngle)
Case part_RHand
x = Cx + _
UpperArmLength * Cos(RShoulderAngle) + _
LowerArmLength * Cos(RElbowAngle)
y = Cy + 2 * HeadRadius + NeckLength - _
UpperArmLength * Sin(RShoulderAngle) - _
LowerArmLength * Sin(RElbowAngle)
Case part_Hips
x = Cx
y = Cy + 2 * HeadRadius + BodyLength
Case part_LKnee
x = Cx + _
UpperLegLength * Cos(LHipAngle)
y = Cy + 2 * HeadRadius + BodyLength - _
UpperLegLength * Sin(LHipAngle)
Case part_RKnee
x = Cx + _
UpperLegLength * Cos(RHipAngle)
y = Cy + 2 * HeadRadius + BodyLength - _
UpperLegLength * Sin(RHipAngle)
Case part_LFoot
x = Cx + _
UpperLegLength * Cos(LHipAngle) + _
LowerLegLength * Cos(LKneeAngle)
y = Cy + 2 * HeadRadius + BodyLength - _
UpperLegLength * Sin(LHipAngle) - _
LowerLegLength * Sin(LKneeAngle)
Case part_RFoot
x = Cx + _
UpperLegLength * Cos(RHipAngle) + _
LowerLegLength * Cos(RKneeAngle)
y = Cy + 2 * HeadRadius + BodyLength - _
UpperLegLength * Sin(RHipAngle) - _
LowerLegLength * Sin(RKneeAngle)
End Select
End Sub
' Draw the robot.
Public Sub Draw(pic As PictureBox, handles As Boolean)
Dim x1 As Integer
Dim y1 As Integer
Dim x2 As Integer
Dim y2 As Integer
Dim x3 As Integer
Dim y3 As Integer
' Draw the head.
x1 = Cx
y1 = Cy + HeadRadius
pic.Circle (x1, y1), HeadRadius
If handles Then _
pic.Line (Cx - Near, Cy - Near)- _
Step(Near2, Near2), , BF
' Draw the body.
y1 = y1 + HeadRadius
pic.Line (x1, y1)-Step(0, BodyLength)
' Draw the left arm.
y1 = y1 + NeckLength
x2 = x1 + UpperArmLength * Cos(LShoulderAngle)
y2 = y1 - UpperArmLength * Sin(LShoulderAngle)
pic.Line (x1, y1)-(x2, y2)
x3 = x2 + LowerArmLength * Cos(LElbowAngle)
y3 = y2 - LowerArmLength * Sin(LElbowAngle)
pic.Line -(x3, y3)
If handles Then _
pic.Line (x2 - Near, y2 - Near)- _
Step(Near2, Near2), , BF
If handles Then _
pic.Line (x3 - Near, y3 - Near)- _
Step(Near2, Near2), , BF
' Draw the right arm.
x2 = x1 + UpperArmLength * Cos(RShoulderAngle)
y2 = y1 - UpperArmLength * Sin(RShoulderAngle)
pic.Line (x1, y1)-(x2, y2)
x3 = x2 + LowerArmLength * Cos(RElbowAngle)
y3 = y2 - LowerArmLength * Sin(RElbowAngle)
pic.Line -(x3, y3)
If handles Then _
pic.Line (x2 - Near, y2 - Near)- _
Step(Near2, Near2), , BF
If handles Then _
pic.Line (x3 - Near, y3 - Near)- _
Step(Near2, Near2), , BF
' Draw the left leg.
y1 = y1 + TrunkLength
x2 = x1 + UpperLegLength * Cos(LHipAngle)
y2 = y1 - UpperLegLength * Sin(LHipAngle)
pic.Line (x1, y1)-(x2, y2)
x3 = x2 + LowerLegLength * Cos(LKneeAngle)
y3 = y2 - LowerLegLength * Sin(LKneeAngle)
pic.Line -(x3, y3)
If handles Then _
pic.Line (x2 - Near, y2 - Near)- _
Step(Near2, Near2), , BF
If handles Then _
pic.Line (x3 - Near, y3 - Near)- _
Step(Near2, Near2), , BF
' Draw the right leg.
x2 = x1 + UpperLegLength * Cos(RHipAngle)
y2 = y1 - UpperLegLength * Sin(RHipAngle)
pic.Line (x1, y1)-(x2, y2)
x3 = x2 + LowerLegLength * Cos(RKneeAngle)
y3 = y2 - LowerLegLength * Sin(RKneeAngle)
pic.Line -(x3, y3)
If handles Then _
pic.Line (x2 - Near, y2 - Near)- _
Step(Near2, Near2), , BF
If handles Then _
pic.Line (x3 - Near, y3 - Near)- _
Step(Near2, Near2), , BF
End Sub
' Move the control point to this location.
Public Sub MoveControlPoint(part As Integer, Ax As Integer, Ay As Integer, x As Integer, y As Integer)
Select Case part
Case part_Head
Cx = x
Cy = y
Case part_Lelbow
LShoulderAngle = Arctan2(x - Ax, Ay - y)
Case part_RElbow
RShoulderAngle = Arctan2(x - Ax, Ay - y)
Case part_LHand
LElbowAngle = Arctan2(x - Ax, Ay - y)
Case part_RHand
RElbowAngle = Arctan2(x - Ax, Ay - y)
Case part_LKnee
LHipAngle = Arctan2(x - Ax, Ay - y)
Case part_RKnee
RHipAngle = Arctan2(x - Ax, Ay - y)
Case part_LFoot
LKneeAngle = Arctan2(x - Ax, Ay - y)
Case part_RFoot
RKneeAngle = Arctan2(x - Ax, Ay - y)
End Select
End Sub
' Initialize the robot's parameters.
Public Sub SetParameters(x As Integer, y As Integer, ls As Single, rs As Single, le As Single, re As Single, lh As Single, rh As Single, lk As Single, rk As Single)
